#include <math.h>
#include "TinySLAM.h"

ts_position_t 
ts_close_loop_position(ts_state_t *state, ts_sensor_data_t *sensor_data, ts_map_t *loop_close_map, ts_position_t *start_position, int *q)
{
    ts_scan_t scan;
    int quality;
    ts_position_t lc_position;

    ts_build_scan(sensor_data, &scan, state, 1);
    lc_position = ts_monte_carlo_search(&state->randomizer, &scan, loop_close_map, start_position, 600, 20, 100000, &quality);
    if (q) *q = quality;
    return lc_position;
}
    
void 
ts_close_loop_trajectory(ts_sensor_data_t *sensor_data, int maxscans, 
        ts_position_t *startpos, ts_position_t *close_loop_position)
{
    int i, j;
    double weight, theta[2];
    ts_position_t *final_pos;
    for (i = 0; i != maxscans; i++) {
        weight = i / ((double)(maxscans - 1));
        final_pos = &sensor_data[i].position[TS_FINAL_MAP];
        final_pos->x = (1 - weight) * sensor_data[i].position[TS_DIRECTION_FORWARD].x + weight * sensor_data[i].position[TS_DIRECTION_BACKWARD].x;
        final_pos->y = (1 - weight) * sensor_data[i].position[TS_DIRECTION_FORWARD].y + weight * sensor_data[i].position[TS_DIRECTION_BACKWARD].y;
        // Make sure both angles are between 0 and 360
        for (j = 0; j != 2; j++) {
            theta[j] = sensor_data[i].position[j].theta;
            while (theta[j] < 0) theta[j] += 360;
            while (theta[j] >= 360) theta[j] -= 360;
        }
        if (fabs(theta[0] - theta[1]) >= 180)
            if (theta[0] > theta[1]) 
                theta[0] -= 360;
            else
                theta[1] -= 360;
        final_pos->theta = (1 - weight) * theta[0] + weight * theta[1];
    }
}


